#include <U8g2lib.h>
#include <Wire.h>
#include <LobotServoController.h>

//////////////////LV8731引脚接线///////////////////////////
// DATA_ABC牛角座-----------ArduinoUNO主板引脚
//     ST(使能)-----------------2
//     ADC---------------------A0
//     VA(STEPA)---------------4
//     VB(STEPB)---------------5
//     VC(STEPC)---------------6
//     5V----------------------5V
//     GND---------------------GND
//     DA(FRA)-----------------8
//     DB(FRB)-----------------9
//     DC(FRC)-----------------10

// DATA_D牛角座--------------ArduinoUNO主板引脚
//     5V-----------------------(与DATA_ABC牛角座5V引脚导通，两者可以只接通1个)
//     GND----------------------(与DATA_ABC牛角座GND引脚导通，两者可以只接通1个)
//     ST-----------------------(与DATA_ABC牛角座ST引脚导通，两者可以只接通1个)
//     DD(FRD)------------------11
//     VD(STEPD)----------------7

//舵机控制板的Tx接Arduino的Rx
//舵机控制板的Rx接Arduino的Tx
//舵机控制板的GND接Arduino的GND   串口接法

/////////////////////////////////////////////////////////////////////////！！！！！！！！！！！接线一定要看！！！！！！！！！！！

//定义引脚名称
#define ST_ABC 2 //使能引脚，该引脚时高电平才允许控制电机，低电平时电机停止
//#define ST_D   12  //使能引脚，与ST_ABC导通
#define ADC A0  //用于测电源电压
#define STEPA 4 //电机A转速控制引脚，输出高频方波控制转速，频率越高速度越快
#define STEPB 5 //电机B转速控制引脚，输出高频方波控制转速，频率越高速度越快
#define STEPC 6 //电机C转速控制引脚，输出高频方波控制转速，频率越高速度越快
#define STEPD 7 //电机D转速控制引脚，输出高频方波控制转速，频率越高速度越快
#define FRA 8   //电机A转向控制引脚，高电位平时电机A顺时针旋转，低电位平时电机A逆时针旋转
#define FRB 9   //电机B转向控制引脚，高电位平时电机B顺时针旋转，低电位平时电机B逆时针旋转
#define FRC 10  //电机C转向控制引脚，高电位平时电机C顺时针旋转，低电位平时电机C逆时针旋转
#define FRD 11  //电机D转向控制引脚，高电位平时电机D顺时针旋转，低电位平时电机D逆时针旋转

U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/SCL, /* data=*/SDA, /* reset=*/U8X8_PIN_NONE);

#include <LobotServoController.h> //机械手舵机的库，记得先加载库再验证

LobotServoController robotarm(Serial2);

void setup()
{
  // put your setup code here, to run once:
  Serial2.begin(9600); //舵机控制板连接的串口2
  Scaner_Init();       //扫描模块连接串口3
  OLED_Init();         //初始化OLED
  pinMode(ST_ABC, OUTPUT);
  pinMode(ADC, INPUT); //设置为输入引脚，读取电压模拟量
  pinMode(STEPA, OUTPUT);
  pinMode(STEPB, OUTPUT);
  pinMode(STEPC, OUTPUT);
  pinMode(FRA, OUTPUT);
  pinMode(FRB, OUTPUT);
  pinMode(FRC, OUTPUT);
  pinMode(FRD, OUTPUT);
  pinMode(STEPD, OUTPUT);

  //初始化LV8731电机驱动模块
  digitalWrite(ST_ABC, 1);
  digitalWrite(STEPA, 0);
  digitalWrite(STEPB, 0);
  digitalWrite(STEPC, 0);
  digitalWrite(STEPD, 0);
  digitalWrite(FRA, 0); //电机A旋转方向为逆时针
  digitalWrite(FRB, 0); //电机B旋转方向为逆时针
  digitalWrite(FRC, 0); //电机C旋转方向为逆时针
  digitalWrite(FRD, 0); //电机D旋转方向为逆时针

  pinMode(13, OUTPUT);
  while (!Serial2);

  digitalWrite(13, HIGH); // 13号口外接指示灯？没有就算了，或者改成内部led？
}

void loop()
{
  // put your main code here, to run repeatedly:
  stay(3000);
  qr_code = Scan_QR(); //扫描二维码
  OLED_Display(qr_code);//显示二维码
  delay(15000);
  go();
  while(1); //防止撞墙
}

void go() //内部数值初拟，待现场调试修正
{
  fw(15600, 300); //执行前进到取料地
  rotr(6000, 300);  
  //机械手取料，动作组
  robotarm.runActionGroup(1,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);
  robotarm.runActionGroup(2,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);
  robotarm.runActionGroup(3,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);

  fw(15600, 300); //前进到放料区
  rotr(6000, 300);
  robotarm.runActionGroup(6,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);
  robotarm.runActionGroup(5,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);
  robotarm.runActionGroup(4,1);
  delay(15000);
  robotarm.stopActionGroup();
  delay(1000);

 fw(15600, 300); //到终点
}

void stay(int st)
{

  digitalWrite(STEPA, 0);
  digitalWrite(STEPB, 0);
  digitalWrite(STEPC, 0);
  digitalWrite(STEPD, 0);
  delay(st);
}
void fw(int steps, int dt) //前
{

  digitalWrite(FRA, LOW);  //左前角电机 high靠自己的方向
  digitalWrite(FRB, LOW);  //左后角电机 high靠自己的方向
  digitalWrite(FRC, HIGH); //     //右上角电机 high背离自己的方向
  digitalWrite(FRD, HIGH); //右下角电机 high背离自己的方向
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void bw(int steps, int dt) //后
{

  digitalWrite(FRA, LOW);
  digitalWrite(FRB, HIGH);
  digitalWrite(FRC, HIGH);
  digitalWrite(FRD, LOW);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void rotl(int steps, int dt) //原地左转
{

  digitalWrite(FRA, HIGH);
  digitalWrite(FRB, HIGH);
  digitalWrite(FRC, HIGH);
  digitalWrite(FRD, HIGH);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void rotr(int steps, int dt) //原地右转
{

  digitalWrite(FRA, LOW);
  digitalWrite(FRB, LOW);
  digitalWrite(FRC, LOW);
  digitalWrite(FRD, LOW);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void shiftl(int steps, int dt) //左平移
{

  digitalWrite(FRA, HIGH);
  digitalWrite(FRB, LOW);
  digitalWrite(FRC, HIGH);
  digitalWrite(FRD, LOW);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void shiftr(int steps, int dt) //右平移
{

  digitalWrite(FRA, LOW);
  digitalWrite(FRB, HIGH);
  digitalWrite(FRC, LOW);
  digitalWrite(FRD, HIGH);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void fl45(int steps, int dt) //左前45度
{

  digitalWrite(FRA, HIGH);
  digitalWrite(FRB, LOW);
  digitalWrite(FRC, 0);
  digitalWrite(FRD, 0);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, 0);
    digitalWrite(STEPD, 0);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, 0);
    digitalWrite(STEPD, 0);
    delayMicroseconds(dt);
  }
}
void fr45(int steps, int dt) //右前45度
{

  digitalWrite(FRA, 0);
  digitalWrite(FRB, 0);
  digitalWrite(FRC, LOW);
  digitalWrite(FRD, HIGH);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, 0);
    digitalWrite(STEPB, 0);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, 0);
    digitalWrite(STEPB, 0);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void bl45(int steps, int dt) //左后45度
{

  digitalWrite(FRA, 0);
  digitalWrite(FRB, 0);
  digitalWrite(FRC, HIGH);
  digitalWrite(FRD, LOW);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, 0);
    digitalWrite(STEPB, 0);
    digitalWrite(STEPC, HIGH);
    digitalWrite(STEPD, HIGH);
    delayMicroseconds(dt);
    digitalWrite(STEPA, 0);
    digitalWrite(STEPB, 0);
    digitalWrite(STEPC, LOW);
    digitalWrite(STEPD, LOW);
    delayMicroseconds(dt);
  }
}
void br45(int steps, int dt) //右后45度
{

  digitalWrite(FRA, LOW);
  digitalWrite(FRB, HIGH);
  digitalWrite(FRC, 0);
  digitalWrite(FRD, 0);
  for (int x = 0; x < steps; x++)
  {
    digitalWrite(STEPA, HIGH);
    digitalWrite(STEPB, HIGH);
    digitalWrite(STEPC, 0);
    digitalWrite(STEPD, 0);
    delayMicroseconds(dt);
    digitalWrite(STEPA, LOW);
    digitalWrite(STEPB, LOW);
    digitalWrite(STEPC, 0);
    digitalWrite(STEPD, 0);
    delayMicroseconds(dt);
  }
}
